#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Joy
import os
import subprocess
# from kezi_msgs.msg import MotorControlCmd
import time

# moter_pub = rospy.Publisher("/kezi/moter_control",MotorControlCmd,queue_size=1)
last_publish_time = time.time() 
class JoyInfo:
    def __init__(self):
        self.Button_A = 0  # 初始化默认值
        self.Button_B = 0
        self.Button_X = 0
        self.Button_Y = 0
        self.Button_LB = 0
        self.Button_RB = 0
        self.Button_LT = 0
        self.Button_RT = 0
        self.Button_back = 0
        # 组合键
        self.Button_LB_A = 0
        self.Button_LB_B = 0
        self.Button_LB_back = 0

        self.Axes_L_FB = 0.0


joy_info = JoyInfo()
# cmd = MotorControlCmd()

def joy_callback(data):
    """
    处理接收到的Joy消息的回调函数
    """
    global joy_info, last_publish_time

    if (data.buttons[0] != joy_info.Button_A):
        joy_info.Button_A = data.buttons[0]
    if (data.buttons[1] != joy_info.Button_B):
        joy_info.Button_B = data.buttons[1]
    if (data.buttons[3] != joy_info.Button_X):
        joy_info.Button_X = data.buttons[3]
    if (data.buttons[4] != joy_info.Button_Y):
        joy_info.Button_Y = data.buttons[4]
    if (data.buttons[6] != joy_info.Button_LB):
        joy_info.Button_LB = data.buttons[6]
    if (data.buttons[7] != joy_info.Button_RB):
        joy_info.Button_RB = data.buttons[7]
    if (data.buttons[10] != joy_info.Button_back):
        joy_info.Button_back = data.buttons[10]
        

    # if (joy_info.Button_LB):
    #     # current_time = time.time()
    #     # if current_time - last_publish_time >= 0.3: 
    #     if (data.axes[1] != 0):
    #         joy_info.Axes_L_FB = data.axes[1]
    #         MotorControlCmd.mode = 2
    #         MotorControlCmd.id = 1
    #         MotorControlCmd.velocity = joy_info.Axes_L_FB * 6.0
    #         moter_pub.publish(cmd)
    #     else :
    #         MotorControlCmd.velocity = joy_info.Axes_L_FB * 0.0
    #         moter_pub.publish(cmd)
    #         # last_publish_time = current_time

    # LB + A 组合键
    if (joy_info.Button_LB & joy_info.Button_A):
        joy_info.Button_LB_A = 1
        joy_info.Button_LB_B = 1
        result = subprocess.run(["bash", "/home/ukz/process/kit_ws/src/kezi_joy/shell/start_node.sh", "start"], 
                       capture_output=True, text=True)
        print(result.stdout)  # 输出: Hello from bash

    else :
        joy_info.Button_LB_A = 0

    # LB + B 组合键
    if (joy_info.Button_LB & joy_info.Button_B):
        joy_info.Button_LB_B = 1
        result = subprocess.run(["bash", "/home/ukz/process/kit_ws/src/kezi_joy/shell/start_node.sh", "stop"], 
                       capture_output=True, text=True)
        print(result.stdout)  # 输出: Hello from bash
    else :
        joy_info.Button_LB_B = 0

    print_joy_info()

    # 打印接收到的消息内容
    # rospy.loginfo("Received Joy message:")
    # rospy.loginfo("Axes: %s", data.axes)
    # rospy.loginfo("Buttons: %s", data.buttons)

def joy_subscriber():
    """
    初始化ROS节点并订阅joy话题
    """
    # 初始化节点，命名为'joy_subscriber'
    rospy.init_node('joy_subscriber', anonymous=True)
    
    # 订阅'joy'话题，消息类型为Joy，回调函数为joy_callback
    rospy.Subscriber("joy", Joy, joy_callback)
    
    # 保持节点运行
    rospy.spin()

def print_joy_info():
    os.system('cls' if os.name == 'nt' else 'clear')
    print("---------- Joy Info ----------")
    print("Button_A:" + str(joy_info.Button_A))
    print("Button_B:" + str(joy_info.Button_B))
    print("Button_X:" + str(joy_info.Button_X))
    print("Button_Y:" + str(joy_info.Button_Y))
    print("Button_LB:" + str(joy_info.Button_LB))
    print("Button_RB:" + str(joy_info.Button_RB))
    print("Button_back:" + str(joy_info.Button_back))
    print("Button_LB_A:" + str(joy_info.Button_LB_A))
    print("Button_LB_B:" + str(joy_info.Button_LB_B))
    

# def run_plotjugger()
    


if __name__ == '__main__':
    try:
        joy_subscriber()
    except rospy.ROSInterruptException:
        pass